
// ===============================================================================================================
// -*- C++ -*-
//
// Vector.cpp - 2D/3D vector math.
//
// Copyright (c) 2011 Guilherme R. Lampert
// guilherme.ronaldo.lampert@gmail.com
//
// This code is licenced under the MIT license.
//
// This software is provided "as is" without express or implied
// warranties. You may freely copy and compile this source into
// applications you distribute provided that the copyright text
// above is included in the resulting source code.
//
// ===============================================================================================================

#include <MathLib.hpp>

namespace MathLib {

// == Vec2f Class Implementation ==

Vec2f::Vec2f(float X, float Y) : x(X), y(Y)
{
}

Vec2f::Vec2f(const Vec2f & r) : x(r.x), y(r.y)
{
}

Vec2f & Vec2f::operator = (float Val)
{
	x = Val;
	y = Val;
	return (*this);
}

Vec2f & Vec2f::operator = (const Vec2f & r)
{
	x = r.x;
	y = r.y;
	return (*this);
}

bool Vec2f::operator == (const Vec2f & r) const
{
	return ((x == r.x) && (y == r.y));
}

bool Vec2f::operator != (const Vec2f & r) const
{
	return ((x != r.x) && (y != r.y));
}

Vec2f Vec2f::Lerp(const Vec2f & vec, float t) const
{
	return (Vec2f((x + t * (vec.x - x)), (y + t * (vec.y - y))));
}

float Vec2f::DotProduct(const Vec2f & r) const
{
	return ((x * r.x) + (y * r.y));
}

float Vec2f::Length(void) const
{
	return (MathLib::Sqrt((x * x) + (y * y)));
}

void Vec2f::Normalize(void)
{
	const float invLen = MathLib::InvSqrt((x * x) + (y * y));
	x *= invLen;
	y *= invLen;
}

void Vec2f::Scale(float factor)
{
	x *= factor;
	y *= factor;
}

void Vec2f::Negate(void)
{
	x = -x;
	y = -y;
}

// == Vec3f Class Implementation ==

Vec3f::Vec3f(float X, float Y, float Z) : x(X), y(Y), z(Z)
{
}

Vec3f::Vec3f(const Vec3f & r) : x(r.x), y(r.y), z(r.z)
{
}

Vec3f & Vec3f::operator = (float Val)
{
	x = Val;
	y = Val;
	z = Val;
	return (*this);
}

Vec3f & Vec3f::operator = (const Vec3f & r)
{
	x = r.x;
	y = r.y;
	z = r.z;
	return (*this);
}

bool Vec3f::operator == (const Vec3f & r) const
{
	return ((x == r.x) && (y == r.y) && (z == r.z));
}

bool Vec3f::operator != (const Vec3f & r) const
{
	return ((x != r.x) && (y != r.y) && (z != r.z));
}

Vec3f Vec3f::operator + (const Vec3f & vec) const
{
	return (Vec3f(x + vec.x, y + vec.y, z + vec.z));
}

Vec3f Vec3f::operator - (const Vec3f & vec) const
{
	return (Vec3f(x - vec.x, y - vec.y, z - vec.z));
}

Vec3f Vec3f::operator * (float scalar) const
{
	return (Vec3f(x * scalar, y * scalar, z * scalar));
}

Vec3f Vec3f::operator / (float scalar) const
{
	return (Vec3f(x / scalar, y / scalar, z / scalar));
}

Vec3f Vec3f::operator - (void) const
{
	return (Vec3f(-x, -y, -z));
}

Vec3f & Vec3f::operator += (const Vec3f & vec)
{
	x += vec.x;
	y += vec.y;
	z += vec.z;
	return (*this);
}

Vec3f & Vec3f::operator -= (const Vec3f & vec)
{
	x -= vec.x;
	y -= vec.y;
	z -= vec.z;
	return (*this);
}

Vec3f & Vec3f::operator *= (float scalar)
{
	x *= scalar;
	y *= scalar;
	z *= scalar;
	return (*this);
}

Vec3f & Vec3f::operator /= (float scalar)
{
	x /= scalar;
	y /= scalar;
	z /= scalar;
	return (*this);
}

Vec3f operator * (float scalar, const Vec3f & vec)
{
	return (Vec3f(scalar * vec.x, scalar * vec.y, scalar * vec.z));
}

Vec3f operator / (float scalar, const Vec3f & vec)
{
	return (Vec3f(scalar / vec.x, scalar / vec.y, scalar / vec.z));
}

float Vec3f::DotProduct(const Vec3f & r) const
{
	return ((x * r.x) + (y * r.y) + (z * r.z));
}

float Vec3f::Length(void) const
{
	return (MathLib::Sqrt((x * x) + (y * y) + (z * z)));
}

Vec3f Vec3f::CrossProduct(const Vec3f & vec) const
{
	return (Vec3f(((y * vec.z) - (z * vec.y)),
				 ((z * vec.x) - (x * vec.z)),
				 ((x * vec.y) - (y * vec.x))));
}

float Vec3f::Angle(const Vec3f & v) const
{
	const float n = MathLib::InvSqrt((x * x) + (y * y) + (z * z)) * ((v.x * v.x) + (v.y * v.y) + (v.z * v.z));
	const float dot = (x * v.x) + (y * v.y) + (z * v.z) * n;

	// Avoid NaN issues
	if (dot <= -1.0f)
	{
		return (static_cast<float>(MathLib::PI));
	}
	else if (dot >= 1.0f)
	{
		return (0.0f);
	}
	else
	{
		return (MathLib::ArcCosine(dot));
	}
}

void Vec3f::Rotate(const Vec3f & axis, float angle)
{
	float s, c;
	MathLib::SineCosine(angle, s, c);
	const float k = 1.0f - c;

	const float nx = x * (c + k * axis.x * axis.x) +
					      y * (k * axis.x * axis.y - s * axis.z) +
					      z * (k * axis.x * axis.z + s * axis.y);

	const float ny = x * (k * axis.x * axis.y + s * axis.z) +
					      y * (c + k * axis.y * axis.y) +
					      z * (k * axis.y * axis.z - s * axis.x);

	const float nz = x * (k * axis.x * axis.z - s * axis.y) +
					      y * (k * axis.y * axis.z + s * axis.x) +
					      z * (c + k * axis.z * axis.z);

	// Assign new values:
	x = nx;
	y = ny;
	z = nz;
}

void Vec3f::Normalize(void)
{
	const float invLen = MathLib::InvSqrt((x * x) + (y * y) + (z * z));
	x *= invLen;
	y *= invLen;
	z *= invLen;
}

void Vec3f::Scale(float factor)
{
	x *= factor;
	y *= factor;
	z *= factor;
}

void Vec3f::Negate(void)
{
	x = -x;
	y = -y;
	z = -z;
}

}; // namespace MathLib {}